/**
  * @file       ind_queue.c
  * @author     const_zpc (any question please send mail to const_zpc@163.com)
  * @brief      不定长的队列功能实现【FIFO】
  * @version    V0.3
  * @date       2024-10-13
  * 
  * 
  * ********************************************************************************************************************
  * @details  功能详细说明：
  *           + 元素添加
  *                 + cotIndQueue_Push(cotIndQueue_t *pIndQueue, const void *pdata, size_t length)
  *           + 元素移除
  *                 + cotIndQueue_Pop(cotIndQueue_t *pIndQueue)
  *           + 元素访问
  *                 + cotIndQueue_Front(cotIndQueue_t *pIndQueue, size_t *plength)
  *           + 元素容量
  *                 + cotIndQueue_Empty(cotIndQueue_t *pIndQueue)
  *                 + cotIndQueue_Full(cotIndQueue_t *pIndQueue, size_t length)
  *                 + cotIndQueue_Size(cotIndQueue_t *pIndQueue)
  *           + 内存交换（队列内存交换，减少内存拷贝）
  *                 + cotIndQueue_Swap(cotIndQueue_t *pQueue1, cotIndQueue_t *pQueue2)
  * 
  * ********************************************************************************************************************
  * @par 源码路径: https://gitee.com/const-zpc/cot.git 具体问题及建议可在该网址填写 Issue
  * @par 修改日志: 
  * <table>
  * <tr><th>Date           <th>Version   <th>Author      <th>Description
  * <tr><td>2023-03-26     <td>1.0       <td>const_zpc       <td>初版
  * </table>
  * ********************************************************************************************************************
  */

/* Includes ----------------------------------------------------------------------------------------------------------*/
#include "container/ind_queue.h"
#include <string.h>
#include <stdlib.h>
#define DATA_LENGTH_BYTES   4

static bool CheckLength(cotIndQueue_t *pIndQueue, size_t length)
{
    size_t writeLength = DATA_LENGTH_BYTES + length;

    if (pIndQueue->wIdx > pIndQueue->rIdx)
    {
        if (pIndQueue->limit - pIndQueue->wIdx < DATA_LENGTH_BYTES)
        {
            size_t tmpLength = pIndQueue->limit - pIndQueue->wIdx;
            
            if (pIndQueue->rIdx >= length + tmpLength)
            {
                return true;
            }
        }
        else if (pIndQueue->limit - pIndQueue->wIdx < writeLength)
        {
            if (pIndQueue->rIdx >= length)
            {
                return true;
            }
        }
        else
        {
            return true;
        }
    }
    else if (pIndQueue->wIdx < pIndQueue->rIdx)
    {
        if (pIndQueue->rIdx - pIndQueue->wIdx >= writeLength)
        {
            return true;
        }
    }
    else
    {
        if (pIndQueue->count == 0 && pIndQueue->limit >= writeLength)
        {
            /* 当队列没有数据则需要复位索引，否则造成下一帧写索引超过水平线，导致读取异常 */
            pIndQueue->rIdx = 0;
            pIndQueue->wIdx = 0;
            return true;
        }
    }

    return false;
} 

/**
  * @brief      不定长队列初始化
  * 
  * @param      pIndQueue 不定长队列句柄
  * @param      pBuf      不定长队列buf
  * @param      bufSize   不定长队列buf大小
  */
void cotIndQueue_Init(cotIndQueue_t *pIndQueue, uint8_t *pBuf, size_t bufSize)
{
    pIndQueue->pBuf = pBuf;
    pIndQueue->limit = bufSize;
    pIndQueue->rIdx = 0;
    pIndQueue->wIdx = 0;
    pIndQueue->waterLevel = 0;
    pIndQueue->count = 0;
}

/**
  * @brief      清空队列
  * 
  * @param      pIndQueue 不定长队列句柄
  */
void cotIndQueue_Clear(cotIndQueue_t *pIndQueue)
{
    cotIndQueue_Init(pIndQueue, pIndQueue->pBuf, pIndQueue->limit);
}

/**
  * @brief      判断不定长队列是否为空
  * 
  * @param      pIndQueue 不定长队列句柄
  * @return     true   空
  * @return     false  非空
  */
bool cotIndQueue_Empty(cotIndQueue_t *pIndQueue)
{
    return pIndQueue->count > 0 ? false : true;
}

/**
  * @brief      判断不定长队列剩余空间是否满足指定长度
  * 
  * @param      pIndQueue 不定长队列句柄
  * @param      length  指定长度
  * @return     true  已满，即剩余空间不满足指定长度
  * @return     false 未满，即剩余空间满足指定长度
  */
bool cotIndQueue_Full(cotIndQueue_t *pIndQueue, size_t length)
{
    if (pIndQueue->count > 0 && !CheckLength(pIndQueue, length))
    {
        return true;
    }

    return false;
}

/**
  * @brief      获取不定长队列元素数目
  * 
  * @param      pIndQueue 不定长队列句柄
  * @return     元素数目 
  */
size_t cotIndQueue_Size(cotIndQueue_t *pIndQueue)
{
    return pIndQueue->count;
}


static void PushLengthAndData(cotIndQueue_t *pIndQueue, const uint8_t *pdata, size_t length)
{
    uint8_t *plengthData = (uint8_t *)&length;
    size_t writeLength = DATA_LENGTH_BYTES + length;

    if (pIndQueue->wIdx >= pIndQueue->rIdx)
    {
        if (pIndQueue->limit - pIndQueue->wIdx < DATA_LENGTH_BYTES)
        {
            size_t tmpLength = pIndQueue->limit - pIndQueue->wIdx;
            size_t waterLevel = pIndQueue->wIdx + tmpLength;

            memcpy(&pIndQueue->pBuf[pIndQueue->wIdx], plengthData, tmpLength);
            memcpy(&pIndQueue->pBuf[0], &plengthData[tmpLength], DATA_LENGTH_BYTES - tmpLength);
            memcpy(&pIndQueue->pBuf[DATA_LENGTH_BYTES - tmpLength], pdata, length);
            pIndQueue->wIdx = DATA_LENGTH_BYTES - tmpLength + length;
            pIndQueue->waterLevel = waterLevel;
        }
        else if (pIndQueue->limit - pIndQueue->wIdx < DATA_LENGTH_BYTES + length)
        {
            size_t waterLevel = pIndQueue->wIdx + DATA_LENGTH_BYTES;

            memcpy(&pIndQueue->pBuf[pIndQueue->wIdx], plengthData, DATA_LENGTH_BYTES);
            memcpy(&pIndQueue->pBuf[0], pdata, length);
            pIndQueue->wIdx = length;
            pIndQueue->waterLevel = waterLevel;
        }
        else
        {
            memcpy(&pIndQueue->pBuf[pIndQueue->wIdx], plengthData, DATA_LENGTH_BYTES);
            memcpy(&pIndQueue->pBuf[pIndQueue->wIdx + DATA_LENGTH_BYTES], pdata, length);
            pIndQueue->wIdx += writeLength;
            pIndQueue->waterLevel = pIndQueue->wIdx;
        }
    }
    else
    {
        memcpy(&pIndQueue->pBuf[pIndQueue->wIdx], plengthData, DATA_LENGTH_BYTES);
        memcpy(&pIndQueue->pBuf[pIndQueue->wIdx + DATA_LENGTH_BYTES], pdata, length);
        pIndQueue->wIdx += writeLength;
    }
}

static size_t PullLength(cotIndQueue_t *pIndQueue)
{
    size_t length = 0;
    uint8_t *pdata = (uint8_t *)&length;

    if (pIndQueue->waterLevel - pIndQueue->rIdx < DATA_LENGTH_BYTES)
    {
        size_t tmpLength = pIndQueue->waterLevel - pIndQueue->rIdx;

        memcpy(pdata, &pIndQueue->pBuf[pIndQueue->rIdx], tmpLength);
        memcpy(&pdata[tmpLength], &pIndQueue->pBuf[0], DATA_LENGTH_BYTES - tmpLength);
    }
    else
    {
        memcpy(pdata, &pIndQueue->pBuf[pIndQueue->rIdx], DATA_LENGTH_BYTES);
    }

    return length;
}

static void Pop(cotIndQueue_t *pIndQueue, size_t length)
{
    if (pIndQueue->waterLevel - pIndQueue->rIdx < length)
    {
        size_t tmpLength = pIndQueue->waterLevel - pIndQueue->rIdx;

        pIndQueue->rIdx = length - tmpLength;
    }
    else
    {
        pIndQueue->rIdx += length;
    }
}

/**
  * @brief      返回不定长队列的头部元素
  * 
  * @param[in]  pIndQueue 不定长队列句柄
  * @param[out] plength   返回的元素数据长度
  * @return     返回的元素数据指针
  */
void *cotIndQueue_Front(cotIndQueue_t *pIndQueue, size_t *plength)
{
    if (pIndQueue->count > 0)
    {
        *plength = PullLength(pIndQueue);

        if (pIndQueue->waterLevel == pIndQueue->rIdx + DATA_LENGTH_BYTES)
        {
            return &pIndQueue->pBuf[0];
        }
        else if (pIndQueue->waterLevel > pIndQueue->rIdx + DATA_LENGTH_BYTES)
        {
            return &pIndQueue->pBuf[pIndQueue->rIdx + DATA_LENGTH_BYTES];
        }
        else
        {
            size_t tmpLength = pIndQueue->waterLevel - pIndQueue->rIdx;
            return &pIndQueue->pBuf[tmpLength];
        }
    }
    
    return NULL;
}

/**
  * @brief      往不定长队列尾部追加数据
  * 
  * @param      pIndQueue 不定长队列句柄
  * @param      pdata     数据内容
  * @param      length    数据内容长度
  * @return     0,成功; -1,失败 
  */
int cotIndQueue_Push(cotIndQueue_t *pIndQueue, const void *pdata, size_t length)
{
    if (!CheckLength(pIndQueue, length))
    {
        return -1;
    }

    PushLengthAndData(pIndQueue, (const uint8_t *)pdata, length);
    pIndQueue->count++;
    return 0;
}

/**
  * @brief      不定长队列头部弹出数据
  * 
  * @param      pIndQueue 不定长队列句柄
  * @return     0,成功; -1,失败  
  */
int cotIndQueue_Pop(cotIndQueue_t *pIndQueue)
{
    if (cotIndQueue_Size(pIndQueue) == 0)
    {
        return -1;
    }

    size_t length = PullLength(pIndQueue);
    Pop(pIndQueue, length + DATA_LENGTH_BYTES);
    pIndQueue->count--;
    return 0;
}

/**
  * @brief      两个不定长队列内存进行交换
  * 
  * @param      pQueue1  需要交换的队列句柄1
  * @param      pQueue2  需要交换的队列句柄2
  */
void cotIndQueue_Swap(cotIndQueue_t *pQueue1, cotIndQueue_t *pQueue2)
{
    cotIndQueue_t pTmpQueue = *pQueue2;

    *pQueue2 = *pQueue1;
    *pQueue1 = pTmpQueue;
}
